# -- coding:utf-8 --
#!/usr/bin/python
import sys, os
sys.path.append("/home/avatar/Minibot/04.RobotSamples/01.Arm")
import inspect
from LoadJson import LoadJson
from BwSensorLib import BwSensorLib
from ArmRobot import ArmRobot
from BwRobotLib import BwRobotLib
from MiniBotControlLib import MiniBotControlLib
import time
import BwHelpLib 

# 加载机器人构型配置
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath = os.path.dirname(os.path.abspath(fileName))
config_file = folderPath+"/config.txt"


class ControlGrab:
	def __init__(self):
		# 实例化MiniBotControlLib类，并设置为udp模式
		self.mlib = MiniBotControlLib("udp")
		# 输入ip地址和端口号
		self.ip = "10.10.100.254"
		self.port = 8899
		# 连接机器人,并设置超时时间5s
		self.mlib.Connect(self.ip, self.port, 2000)
		# 实例化BwRobotLib类，并将mlib作为参数传入
		self.robotlib = BwRobotLib(self.mlib)
		# 导入配置文件，获取文件中的模块ID和模块连接位置
		self.module_ids, self.module_ids_loc = self.robotlib.loadConfig(config_file)
		# 扫描当前机器人构型的ID，默认扫描ID段为20-50
		self.module_ids_scan, self.module_ids_loc_scan = self.mlib.ScanId()
		# 判断是否扫描的ID与配置文件导入的ID一致
		res = BwHelpLib.checkListEqual(self.module_ids,self.module_ids_scan)
		if res == False:
			print("\033[31m输入的模块ID与扫描获取的模块ID不一致，请检查！\033[37m")
			print("配置文件中模块ID: ", self.module_ids)
			print("实际扫描的模块ID: ", self.module_ids_scan)
			exit(0)
		else:
			print("输入的模块ID与扫描获取的模块ID一致")
			
		# 实例化操作臂类
		self.armlib = ArmRobot(self.mlib)
		# 将模块ID与操作臂构型绑定
		self.armlib.bindIds(self.module_ids)		
		# 实例化传感器库对象
		self.sensorlib = BwSensorLib(self.mlib)
		# 动作延时时间
		self.wait_time = 1.5
		# 动作速度
		self.speed = 50	
		# 点阵显示图案 
		# 图案分别为：夹子全闭　夹子微张　夹子张开　夹子全张开 无显示
		self.display_data = [[0x00,0x08,0x14,0x3E,0x00],[0x00,0x14,0x22,0xFF,0x00],[0x00,0x22,0x41,0xFF,0x00],
							[0x00,0x41,0x41,0xFF,0x00],[0X00,0X00,0X00,0X00,0X00,0X00,]]
		# 初始化动作	
		self.initRobotMove()

	# 初始化至实验开始位置
	def initRobotMove(self, _time=2):
		angs = [0,-90,90,90,1.2,1.1]
		speed = 20
		self.armlib.position_angles = angs
		self.armlib.pushJointsAngles(speed, self.wait_time)
		self.sensorlib.setMatrixSensor(self.display_data[4]) # 点阵无显示
		time.sleep(_time)

# ####超声波控制夹子张合
	def controlGrab(self):
		while 1:
			# 超声波读数据，有效测量范围为5-20cm处，即地图中的B区-C区中
			dcm = self.sensorlib.readUltrasonicSensor()
			if(dcm>20):
				dcm = 20
			if(dcm<5):
				dcm = 5
			# 超声波距离与夹子张开角度的映射关系		
			self.armlib.position_angles[5] = 6*dcm - 30 
			# 夹子张开角度为0-22.5度
			if(self.armlib.position_angles[5]>=0 and self.armlib.position_angles[5]<=22.5):
				self.sensorlib.setMatrixSensor(self.display_data[0]) # 点阵显示夹子全闭图案
			# 夹子张开角度为22.5-45度
			elif(self.armlib.position_angles[5]>22.5 and self.armlib.position_angles[5]<=45):
				self.sensorlib.setMatrixSensor(self.display_data[1]) # 点阵显示夹子微张图案
			# 夹子张开角度为45-67.5度
			elif(self.armlib.position_angles[5]>45 and self.armlib.position_angles[5]<=67.5):
				self.sensorlib.setMatrixSensor(self.display_data[2]) # 点阵显示张开图案
			# 夹子张开角度为67.5-90度
			else:
				self.sensorlib.setMatrixSensor(self.display_data[3]) # 点阵显示全张开图案
			# 控制夹子张合角度
			self.armlib.pushJointsAngles(self.speed,self.wait_time)
				
				



# 主函数
def main():
	# 实例化超声波传感器夹抓控制类
	control = ControlGrab()
	# 执行超声波传感器夹抓控制功能
	control.controlGrab()


if __name__ == "__main__":
	try:
		main()
	except KeyboardInterrupt:
		print(" Program closed! ")
	pass
